#include "app_car.h"
#include "app_comm.h"
#include "pid.h"
#include "bsp_encoder.h"

#include "motor.h"


extern uint16_t channel[IBUS_USER_CHANNELS];

#define TOP_VALUE           2000    // 摇杆顶部数值
#define BOTTOM_VALUE        1000    // 摇杆底部数值
#define MEDIAN_VALUE        1500    // 摇杆中间数值


#define TOP_DEAD_ZONE       50  // 顶部死区
#define BOTTOM_DEAD_ZONE    50  // 底部死区

#define CAR_MAX_SPEED       200 // 小车最大速度 200rpm


void remote_data_dead_zone_process(float *r_data)
{
     // 1000 ~ 2000 -> -500 ~ 500
    *r_data = *r_data - MEDIAN_VALUE; 

    // 死区处理
    if((*r_data > (-1*BOTTOM_DEAD_ZONE)) && (*r_data < BOTTOM_DEAD_ZONE))
    {
        *r_data = 0;
    }
    else if((*r_data > (0 + BOTTOM_DEAD_ZONE)) && (*r_data < (TOP_VALUE - MEDIAN_VALUE - TOP_DEAD_ZONE))) 
    {
        *r_data = *r_data - BOTTOM_DEAD_ZONE;
    }
    else if((*r_data < (0 - BOTTOM_DEAD_ZONE)) && (*r_data < (BOTTOM_VALUE - MEDIAN_VALUE + TOP_DEAD_ZONE)))
    {
        *r_data = *r_data + BOTTOM_DEAD_ZONE;
    }
    else if(*r_data > (TOP_VALUE - MEDIAN_VALUE - TOP_DEAD_ZONE))
    {
        *r_data = TOP_VALUE - MEDIAN_VALUE - BOTTOM_DEAD_ZONE - TOP_DEAD_ZONE;
    }
    else if(*r_data < (BOTTOM_VALUE - MEDIAN_VALUE + TOP_DEAD_ZONE))
    {
        *r_data = BOTTOM_VALUE - MEDIAN_VALUE + BOTTOM_DEAD_ZONE + TOP_DEAD_ZONE;
    }
}

float r_x,r_y,r_yaw;        // 遥控数据
float convert_para;         // 转换系数
float v_lf,v_lb,v_rf,v_rb;  // 电机数据
uint16_t k_x = 1,k_y = 1,k_yaw = 1; // 通道分配

void chassis_control(void)
{


    // 接收遥控通道数据
    r_x = channel[CHANNEL_R_FB];        // 前后 1000-1500-2000
    r_y = channel[CHANNEL_R_LR];        // 左右 1000-1500-2000
    r_yaw = channel[CHANNEL_L_YAW];     // 旋转 1000-1500-2000
    if(r_x == 0)
        return;


    /* 对接收数据进行处理 中间 死区处理
     由于遥控器摇杆的制作工艺问题, 摇杆的中间位置并不一定是1500
     所以这里对摇杆数据进行处理 
    */

   /*
    |-----------------------|       |-----------------------|
    |           |           |       |  2000 - TOP_DEAD_ZONE |  
    |           |           |       |           |           |
    |           |           |       |1500 + BOTTOM_DEAD_ZONE|
    |-----------+-----------|       |-----------+-----------|
    |           |           |       |1500 - BOTTOM_DEAD_ZONE|
    |           |           |       |           |           |
    |           |           |       |  1000 + TOP_DEAD_ZONE |
    |-----------------------|       |-----------------------|
   */
    // 死区处理
    remote_data_dead_zone_process(&r_x);
    remote_data_dead_zone_process(&r_y);
    remote_data_dead_zone_process(&r_yaw);

    // 处理完之后 将遥控数据投射到小车速度上
    convert_para = (float)(CAR_MAX_SPEED)/(float)(TOP_VALUE - MEDIAN_VALUE - TOP_DEAD_ZONE - BOTTOM_DEAD_ZONE);
    r_x = r_x * convert_para;
    r_y = r_y * convert_para;
    r_yaw = r_yaw * convert_para;
    
    /*
        运动逆解
    |-|           |+| 
    |\|-----------|/|
    |+|     |     |-|
            |
            |
            |
    |-|     |     |+|
    |/|-----------|\|
    |+|           |-|

    麦克纳姆轮 例如 / 轮 是左前和右后方向
                   \ 轮 是左后和右前方向
    通过力的抵消来实现任意方向的运动
    / 
     */

    // rx + 前 - 后
    // ry + 右 - 左
    // ryaw + 右 - 左
    v_lf = ((-1)*k_x*r_x + (-1)*k_y*r_y + (-1)*k_yaw*r_yaw) ;
    v_lb = ((-1)*k_x*r_x +      k_y*r_y + (-1)*k_yaw*r_yaw) ;
    v_rf = (     k_x*r_x + (-1)*k_y*r_y + (-1)*k_yaw*r_yaw) ;
    v_rb = (     k_x*r_x +      k_y*r_y + (-1)*k_yaw*r_yaw) ;
    
    // 设置电机转速
    motor_set_targetval(MOTOR_LF, v_lf);
    motor_set_targetval(MOTOR_LB, v_lb);
    motor_set_targetval(MOTOR_RF, v_rf);
    motor_set_targetval(MOTOR_RB, v_rb);
}



uint8_t gun_event = 0;

void gun_control(void)
{
    if(channel[CHANNEL_SWITCH1] == 2000 && gun_event == 0)
    {
        gun_event = 1;
    }
    
    if(channel[CHANNEL_SWITCH1] == 1000)
    {
        gun_event = 0;
    }

    if(gun_event == 1)
    {
        gun_shoot();
        gun_event = 2;
    }

}







void app_car_task(void)
{
    // 底盘控制
    // 遥控器 ---运动分解--> 电机
    chassis_control();
    // 电机控制
    motor_control();
    // gun控制
    gun_control();

}   
